#include<iostream>
#include<fstream>
#include<sstream>
#include<vector>
#include<set>
#include<list>
#include<boost/foreach.hpp>
#include"math.h"
#include"mpi.h"
#ifdef USE_GPU
#include<CL/cl.hpp>
#include"../common/clsafe.h"
#endif
#include"ssvs_dimensions.h"
#include"../common/main.hpp"
#include"../common/analyzer.hpp"
#include"../common/io.hpp"
#include"../common/utility.hpp"
#include"ssvs.hpp"

using namespace std;

inline float ran(){
  return rand()/(RAND_MAX+1.);
}

model_t::model_t(int subsetsize,int * variables,float likelihood){
  this->subsetsize = subsetsize;
  this->likelihood = likelihood;
  for(int j=0;j<subsetsize;++j){
    this->variables.push_back(variables[j]);
  }
}

model_t:: ~model_t(){
  //cerr<<"Model with likelihood "<<likelihood<<" destroyed\n";
}

void SSVS::init_gpu(int * platform_id, int * device_id, const char * kernel_path){
#ifdef USE_GPU
  cerr<<"Initializing GPU\n";
  if (benchmark) {
    event = new cl::Event;
  }else{
    event = NULL;
  }
  cerr<<"SSVS program initializing GPU with platform id "<<*platform_id<<" and device id "<<*device_id<<" and kernel path "<<kernel_path<<".\n";
  vector<cl::Platform> platforms;
 err = cl::Platform::get(&platforms);
  cerr<<"Platform ID "<<*platform_id<<" has name "<<platforms[*platform_id].getInfo<CL_PLATFORM_NAME>().c_str()<<endl;
  cl_context_properties cps[3] = {CL_CONTEXT_PLATFORM,(cl_context_properties)(platforms[*platform_id])(),0};
  context = new cl::Context(CL_DEVICE_TYPE_GPU,cps);
  devices = context->getInfo<CL_CONTEXT_DEVICES>();
  cerr<<"There are "<<devices.size()<<" devices\n"; 
  cerr<<"Device ID "<<*device_id<<" is of type: ";
  cl_device_type dtype = devices[*device_id].getInfo<CL_DEVICE_TYPE>();
  switch(dtype){
    case CL_DEVICE_TYPE_GPU:
      cerr<<"GPU\n";
      break;
    case CL_DEVICE_TYPE_CPU:
      cerr<<"CPU\n";
      break;
  } 
  commandQueue = new cl::CommandQueue(*context,devices[*device_id],0,&err);
  // create a program object from kernel source
  ifstream ifs(kernel_path);
  clSafe(ifs.is_open()?CL_SUCCESS:-1,"kernel_path not found");
  string source_str(istreambuf_iterator<char>(ifs),(istreambuf_iterator<char>()));
  cl::Program::Sources source(1,make_pair(source_str.c_str(),source_str.length()+1));
  program = new cl::Program(*context,source);
 err = program->build(devices);
  if(err!=CL_SUCCESS){
    cerr<<"Build failed:\n";
    string buffer;
    program->getBuildInfo(devices[0],CL_PROGRAM_BUILD_LOG,&buffer);
    cerr<<buffer<<endl;
  }
  // CREATE KERNELS
  kernel_simple = new cl::Kernel(*program,"simple",&err);
  clSafe(err,"creating kernel simple");
  // CREATE BUFFERS
  buffer_simple_in = new cl::Buffer(*context, CL_MEM_READ_ONLY, sizeof(int) * 1, NULL, &err);
  clSafe(err,"creating buffer in");
  buffer_simple_out = new cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(int) * SSVS_BLOCK_WIDTH, NULL, &err);
  clSafe(err,"creating buffer out");
  // SET KERNEL ARGUMENTS
  int arg;
  int kernelWorkGroupSize;
  arg = 0;
 err = kernel_simple->setArg(arg++, *buffer_simple_in);
  clSafe(err,"set kernel arg for kernel_simple");
 err = kernel_simple->setArg(arg++, *buffer_simple_out);
  clSafe(err,"set kernel arg for kernel_simple");
  kernelWorkGroupSize = kernel_simple->getWorkGroupInfo<CL_KERNEL_WORK_GROUP_SIZE>(devices[0], &err);
  clSafe(err,"get workgroup size kernel simple");
  cerr<<"simple kernel work group size is "<<kernelWorkGroupSize<<endl;
#endif
  cerr<<"GPU initialized\n";
}

void SSVS::init_buffers(){
#ifdef USE_GPU
  // CREATE KERNELS
  kernel_xprimex = new cl::Kernel(*program,"xprimex",&err);
  clSafe(err,"creating kernel xprimex");
  kernel_invert = new cl::Kernel(*program,"invert",&err);
  clSafe(err,"creating kernel invert");
  //kernel_compute_xxx = new cl::Kernel(*program,"compute_xxx",&err);
  //clSafe(err,"creating kernel compute_xxx");
  kernel_compute_beta = new cl::Kernel(*program,"compute_beta",&err);
  clSafe(err,"creating kernel compute_beta");
  kernel_compute_rsq = new cl::Kernel(*program,"compute_rsq",&err);
  clSafe(err,"creating kernel compute_rsq");
  // CREATE BUFFERS
  buffer_designmat = new cl::Buffer(*context, CL_MEM_READ_ONLY, sizeof(float) * samplesize*variables, NULL, &err);
  clSafe(err,"creating buffer design mat");
  buffer_trait = new cl::Buffer(*context, CL_MEM_READ_ONLY, sizeof(float) * samplesize, NULL, &err);
  clSafe(err,"creating buffer trait");
  buffer_subset = new cl::Buffer(*context, CL_MEM_READ_ONLY, sizeof(int) * models * subsetsize, NULL, &err);
  clSafe(err,"creating buffer subset");
  buffer_beta = new cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(float) * models * subsetsize, NULL, &err);
  clSafe(err,"creating buffer beta");
  buffer_xx = new cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(float) * models * subsetsize2, NULL, &err);
  clSafe(err,"creating buffer xx");
  buffer_inv_xx = new cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(float) * models * subsetsize2, NULL, &err);
  clSafe(err,"creating buffer inv xx");
  //buffer_xxx = new cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(float) * models * subsetsize*samplesize, NULL, &err);
  //clSafe(err,"creating buffer xxx");
  buffer_rsq = new cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(float) * models, NULL, &err); clSafe(err,"creating buffer rsq");

  // INITIALIZE BUFFERS
 err = commandQueue->enqueueWriteBuffer(*buffer_designmat, CL_TRUE, 0,  sizeof(float)*samplesize*variables,designmat, NULL, NULL );
  clSafe(err, "write designmat");
 err = commandQueue->enqueueWriteBuffer(*buffer_trait, CL_TRUE, 0,  sizeof(float)*samplesize,trait, NULL, NULL );
  clSafe(err, "write trait");

  // SET KERNEL ARGUMENTS
  int arg;
  int kernelWorkGroupSize;
  arg = 0;
 err = kernel_xprimex->setArg(arg++, models);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, samplesize);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, variables);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, subsetsize);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, subsetsize2);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, *buffer_subset);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, *buffer_designmat);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, *buffer_xx);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, *buffer_inv_xx);
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, cl::__local(sizeof(int)*subsetsize*(SSVS_BLOCK_WIDTH)));
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, cl::__local(sizeof(int)*subsetsize));
  clSafe(err,"set kernel arg for kernel_xprimex");
  //err = kernel_xprimex->setArg(arg++, cl::__local(sizeof(float)*SSVS_BLOCK_WIDTH));
  //clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, cl::__local(sizeof(float)*SSVS_BLOCK_WIDTH));
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, cl::__local(sizeof(float)*subsetsize2));
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, cl::__local(sizeof(float)*subsetsize2));
  clSafe(err,"set kernel arg for kernel_xprimex");
 err = kernel_xprimex->setArg(arg++, cl::__local(sizeof(float)*subsetsize2));
  clSafe(err,"set kernel arg for kernel_xprimex");
  kernelWorkGroupSize = kernel_xprimex->getWorkGroupInfo<CL_KERNEL_WORK_GROUP_SIZE>(devices[0], &err);
  clSafe(err,"get workgroup size kernel xprimex");
  cerr<<"xprimex kernel work group size is "<<kernelWorkGroupSize<<endl;

  arg = 0;
 err = kernel_invert->setArg(arg++, models);
  clSafe(err,"set kernel arg for kernel_invert");
 err = kernel_invert->setArg(arg++, subsetsize);
  clSafe(err,"set kernel arg for kernel_invert");
 err = kernel_invert->setArg(arg++, subsetsize*subsetsize);
  clSafe(err,"set kernel arg for kernel_invert");
 err = kernel_invert->setArg(arg++, *buffer_xx);
  clSafe(err,"set kernel arg for kernel_invert");
 err = kernel_invert->setArg(arg++, *buffer_inv_xx);
  clSafe(err,"set kernel arg for kernel_invert");
 err = kernel_invert->setArg(arg++, cl::__local(sizeof(float)*subsetsize*subsetsize));
  clSafe(err,"set kernel arg for kernel_invert");
 err = kernel_invert->setArg(arg++, cl::__local(sizeof(float)*subsetsize*subsetsize));
  clSafe(err,"set kernel arg for kernel_invert");
 err = kernel_invert->setArg(arg++, cl::__local(sizeof(float)*subsetsize*subsetsize));
  clSafe(err,"set kernel arg for kernel_invert");
  kernelWorkGroupSize = kernel_invert->getWorkGroupInfo<CL_KERNEL_WORK_GROUP_SIZE>(devices[0], &err);
  clSafe(err,"get workgroup size kernel invert");
  cerr<<"invert kernel work group size is "<<kernelWorkGroupSize<<endl;
  
  //arg = 0;
  //err = kernel_compute_xxx->setArg(arg++, models);
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, samplesize);
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, subsetsize);
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, subsetsize2);
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, *buffer_subset);
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, *buffer_designmat);
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, *buffer_inv_xx);
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, *buffer_xxx);
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, cl::__local(sizeof(float)*subsetsize2));
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, cl::__local(sizeof(float)*subsetsize*SSVS_BLOCK_WIDTH));
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //err = kernel_compute_xxx->setArg(arg++, cl::__local(sizeof(float)*subsetsize*SSVS_BLOCK_WIDTH));
  //clSafe(err,"set kernel arg for kernel_compute_xxx");
  //kernelWorkGroupSize = kernel_compute_xxx->getWorkGroupInfo<CL_KERNEL_WORK_GROUP_SIZE>(devices[0], &err);
  //clSafe(err,"get workgroup size kernel compute_xxx");
  //cerr<<"compute_xxx kernel work group size is "<<kernelWorkGroupSize<<endl;

  arg = 0;
 err = kernel_compute_beta->setArg(arg++, models);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, samplesize);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, subsetsize);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, subsetsize2);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, *buffer_subset);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, *buffer_designmat);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, *buffer_inv_xx);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, *buffer_trait);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, *buffer_beta);
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(int)*subsetsize));
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*samplesize));
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*subsetsize2));
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*subsetsize*SSVS_BLOCK_WIDTH));
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*subsetsize*SSVS_BLOCK_WIDTH));
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*subsetsize*SSVS_BLOCK_WIDTH));
  clSafe(err,"set kernel arg for kernel_compute_beta");
 err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*subsetsize));
  clSafe(err,"set kernel arg for kernel_compute_beta");
  kernelWorkGroupSize = kernel_compute_beta->getWorkGroupInfo<CL_KERNEL_WORK_GROUP_SIZE>(devices[0], &err);
  clSafe(err,"get workgroup size kernel compute_beta");
  cerr<<"compute_beta kernel work group size is "<<kernelWorkGroupSize<<endl;

  //arg = 0;
  //err = kernel_compute_beta->setArg(arg++, models);
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, samplesize);
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, subsetsize);
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, subsetsize*subsetsize);
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, *buffer_xxx);
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, *buffer_trait);
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, *buffer_beta);
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*samplesize));
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*SSVS_BLOCK_WIDTH));
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //err = kernel_compute_beta->setArg(arg++, cl::__local(sizeof(float)*subsetsize));
  //clSafe(err,"set kernel arg for kernel_compute_beta");
  //kernelWorkGroupSize = kernel_compute_beta->getWorkGroupInfo<CL_KERNEL_WORK_GROUP_SIZE>(devices[0], &err);
  //clSafe(err,"get workgroup size kernel compute_beta");
  //cerr<<"compute_beta kernel work group size is "<<kernelWorkGroupSize<<endl;

  arg = 0;
 err = kernel_compute_rsq->setArg(arg++, models);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, samplesize);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, subsetsize);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, subsetsize2);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, trait_mean);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, tss);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, *buffer_subset);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, *buffer_beta);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, *buffer_designmat);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, *buffer_rsq);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, *buffer_trait);
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, cl::__local(sizeof(int)*subsetsize));
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, cl::__local(sizeof(float)*samplesize));
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, cl::__local(sizeof(float)*samplesize));
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, cl::__local(sizeof(float)*SSVS_BLOCK_WIDTH));
  clSafe(err,"set kernel arg for kernel_compute_rsq");
 err = kernel_compute_rsq->setArg(arg++, cl::__local(sizeof(float)*subsetsize));
  clSafe(err,"set kernel arg for kernel_compute_rsq");
  kernelWorkGroupSize = kernel_compute_rsq->getWorkGroupInfo<CL_KERNEL_WORK_GROUP_SIZE>(devices[0], &err);
  clSafe(err,"get workgroup size kernel compute_rsq");
  cerr<<"compute_rsq kernel work group size is "<<kernelWorkGroupSize<<endl;
#endif
}

void SSVS::run_simple(int * scaler, int * return_vec){
#ifdef USE_GPU
  cl_int err;
  //cerr<<"Scaler: "<<scaler[0];
  for(int i=0;i<256;++i){
    //return_vec[i]*=scaler[0];
    //cerr<<i<<":"<<return_vec[i]<<" ";
  }
  cerr<<endl;
 err = commandQueue->enqueueWriteBuffer(*buffer_simple_in, CL_TRUE, sizeof(int)*0,  sizeof(int)*1, scaler, NULL, NULL );
  clSafe(err, "write scaler");
 err = commandQueue->enqueueWriteBuffer(*buffer_simple_out, CL_TRUE, sizeof(int)*0,  sizeof(int)*SSVS_BLOCK_WIDTH, return_vec, NULL, NULL );
  clSafe(err, "write vector");
 err = commandQueue->enqueueNDRangeKernel(*kernel_simple,cl::NullRange,cl::NDRange(SSVS_BLOCK_WIDTH,1),cl::NDRange(SSVS_BLOCK_WIDTH,1),NULL,NULL);
  clSafe(err,"launch simple kernel");
  cerr<<"launched kernel\n";
 err = commandQueue->enqueueReadBuffer(*buffer_simple_out, CL_TRUE, 0, SSVS_BLOCK_WIDTH*sizeof(int),return_vec);
  clSafe(err, "read test vec");
  cerr<<"Read buffer\n";
#endif
}

void SSVS::invert_xprimex(){
  bool debug = false;
  if (run_cpu){
    double start=clock();
    for(int model=0;model<models;++model){
      //float vec2[samplesize];
      for(int j=0;j<subsetsize;++j){
        float * vec1 = designmat+(subset[model*subsetsize+j]*samplesize);
        for(int k=0;k<subsetsize;++k){
          float * vec2 = designmat+(subset[model*subsetsize+k]*samplesize);
          float dotprod=0;
          for(int i=0;i<samplesize;++i){
            dotprod+=vec1[i]*vec2[i];
          }
          xx[model*subsetsize2 + j*subsetsize+k] = dotprod;
        }
      }
      if (debug){
        cerr<<"CPU:\n";
        for(int j=0;j<subsetsize;++j){
          for(int k=0;k<subsetsize;++k){
            cerr<<" "<<xx[model*subsetsize2 + j*subsetsize+k];
          }
          cerr<<endl;
        }
      }

      float chol_xx[subsetsize2];
      // copy everything over;
      for(int i=0;i<subsetsize;++i){
        for(int j=0;j<subsetsize;++j){
          chol_xx[i*subsetsize+j] = xx[model*subsetsize2+i*subsetsize+j];
          inv_xx[model*subsetsize2+i*subsetsize+j] = i==j?1:0;
        }
      } 
      for(int i=0;i<subsetsize;++i){
        for(int j=i;j<subsetsize;++j){
          float sum = xx[model*subsetsize2+i*subsetsize+j];
          for(int k=0;k<i;++k){
            //sum-=chol_xx[k*subsetsize+i]*chol_xx[k*subsetsize+j];
            sum-=chol_xx[i*subsetsize+k]*chol_xx[j*subsetsize+k];
          }
          //chol_xx[i*subsetsize+j] = (i==j)?sqrt(sum):sum/chol_xx[i*subsetsize+i];
          chol_xx[j*subsetsize+i] = (i==j)?sqrt(sum):sum/chol_xx[i*subsetsize+i];
        }
      }
      if(debug){
        cerr<<"Cholesky CPU:\n";
        for(int j=0;j<subsetsize;++j){
          for(int k=0;k<subsetsize;++k){
            cerr<<" "<<chol_xx[j*subsetsize+k];
          }
          cerr<<endl;
        }
      }
      //return;
      for(int i=0;i<subsetsize;++i){
        for(int j=0;j<subsetsize;++j){
          float sum = inv_xx[model*subsetsize2+i*subsetsize+j];
          for(int k=0;k<j;++k){
            sum-=chol_xx[j*subsetsize+k]*inv_xx[model*subsetsize2+i*subsetsize+k];
          }
          inv_xx[model*subsetsize2+i*subsetsize+j] = sum/chol_xx[j*subsetsize+j];
        }
        for(int j=subsetsize-1;j>=0;--j){
          float sum = inv_xx[model*subsetsize2+i*subsetsize+j];
          for(int k=subsetsize-1;k>j;--k){
            sum-=chol_xx[k*subsetsize+j]*inv_xx[model*subsetsize2+i*subsetsize+k];
          }
          inv_xx[model*subsetsize2+i*subsetsize+j] = sum/chol_xx[j*subsetsize+j];
        }
      }
      if (debug){
        cerr<<"Inversion CPU:\n";
        for(int j=0;j<subsetsize;++j){
          for(int k=0;k<subsetsize;++k){
            cerr<<" "<<inv_xx[model*subsetsize2+j*subsetsize+k];
          }
          cerr<<endl;
        }
      }
    }
    if(benchmark)cerr<<"xprimex CPU time: "<<(clock()-start)/CLOCKS_PER_SEC<<endl;
  }
  if (run_gpu){
    double start=clock();
    //err = commandQueue->enqueueWriteBuffer(*buffer_simple_in, CL_TRUE, sizeof(int)*0,  sizeof(int)*1, scaler, NULL, NULL );
    //clSafe(err, "write scaler");
   err = commandQueue->enqueueNDRangeKernel(*kernel_xprimex,cl::NullRange,cl::NDRange(models*SSVS_BLOCK_WIDTH),cl::NDRange(SSVS_BLOCK_WIDTH),NULL,event);
    clSafe(err,"launch xprimex");
    //cerr<<"launched kernel\n";
//    if (debug){
//     err = commandQueue->enqueueReadBuffer(*buffer_xx, CL_TRUE, 0, models*subsetsize2*sizeof(float),xx);
//      clSafe(err, "read xx");
//      cerr<<"Read buffer\n";
//      for(int model=0;model<models;++model){
//        cerr<<"GPU:\n";
//        for(int j=0;j<subsetsize;++j){
//          for(int k=0;k<subsetsize;++k){
//            cerr<<" "<<xx[model*subsetsize2+j*subsetsize+k];
//          }
//          cerr<<endl;
//        }
//      }
//    }
    if (debug){
     err = commandQueue->enqueueReadBuffer(*buffer_inv_xx, CL_TRUE, 0, models*subsetsize2*sizeof(float),inv_xx);
      clSafe(err, "read inv_xx");
      cerr<<"Read buffer\n";
      for(int model=0;model<models;++model){
        cerr<<"Inversion GPU:\n";
        for(int j=0;j<subsetsize;++j){
          for(int k=0;k<subsetsize;++k){
            cerr<<" "<<inv_xx[model*subsetsize2+j*subsetsize+k];
          }
          cerr<<endl;
        }
      }
    }
    if(benchmark) {
      event->wait();
      cerr<<"xprimex GPU time: "<<(clock()-start)/CLOCKS_PER_SEC<<endl;
    }
  }
  if (debug) exit(0);
}

//void SSVS::invert(){
//  bool debug = false;
//  if (run_cpu){
//    double start=clock();
//    for(int model=0;model<models;++model){
//      float chol_xx[subsetsize2];
//      // copy everything over;
//      for(int i=0;i<subsetsize;++i){
//        for(int j=0;j<subsetsize;++j){
//          chol_xx[i*subsetsize+j] = xx[model*subsetsize2+i*subsetsize+j];
//          inv_xx[model*subsetsize2+i*subsetsize+j] = i==j?1:0;
//        }
//      } 
//      for(int i=0;i<subsetsize;++i){
//        for(int j=i;j<subsetsize;++j){
//          float sum = xx[model*subsetsize2+i*subsetsize+j];
//          for(int k=0;k<i;++k){
//            //sum-=chol_xx[k*subsetsize+i]*chol_xx[k*subsetsize+j];
//            sum-=chol_xx[i*subsetsize+k]*chol_xx[j*subsetsize+k];
//          }
//          //chol_xx[i*subsetsize+j] = (i==j)?sqrt(sum):sum/chol_xx[i*subsetsize+i];
//          chol_xx[j*subsetsize+i] = (i==j)?sqrt(sum):sum/chol_xx[i*subsetsize+i];
//        }
//      }
//      if(debug){
//        cerr<<"Cholesky CPU:\n";
//        for(int j=0;j<subsetsize;++j){
//          for(int k=0;k<subsetsize;++k){
//            cerr<<" "<<chol_xx[j*subsetsize+k];
//          }
//          cerr<<endl;
//        }
//      }
//      //return;
//      for(int i=0;i<subsetsize;++i){
//        for(int j=0;j<subsetsize;++j){
//          float sum = inv_xx[model*subsetsize2+i*subsetsize+j];
//          for(int k=0;k<j;++k){
//            sum-=chol_xx[j*subsetsize+k]*inv_xx[model*subsetsize2+i*subsetsize+k];
//          }
//          inv_xx[model*subsetsize2+i*subsetsize+j] = sum/chol_xx[j*subsetsize+j];
//        }
//        for(int j=subsetsize-1;j>=0;--j){
//          float sum = inv_xx[model*subsetsize2+i*subsetsize+j];
//          for(int k=subsetsize-1;k>j;--k){
//            sum-=chol_xx[k*subsetsize+j]*inv_xx[model*subsetsize2+i*subsetsize+k];
//          }
//          inv_xx[model*subsetsize2+i*subsetsize+j] = sum/chol_xx[j*subsetsize+j];
//        }
//      }
//      if (debug){
//        cerr<<"Inversion CPU:\n";
//        for(int j=0;j<subsetsize;++j){
//          for(int k=0;k<subsetsize;++k){
//            cerr<<" "<<inv_xx[model*subsetsize2+j*subsetsize+k];
//          }
//          cerr<<endl;
//        }
//      }
//    }
//    cerr<<"invert CPU time: "<<(clock()-start)/CLOCKS_PER_SEC<<endl;
//  }
//  if(run_gpu){
//    double start=clock();
//    cl::Event event;
//   err = commandQueue->enqueueNDRangeKernel(*kernel_invert,cl::NullRange,cl::NDRange(models*SSVS_SMALL_BLOCK_WIDTH),cl::NDRange(SSVS_SMALL_BLOCK_WIDTH),NULL,&event);
//    clSafe(err,"launch xprimex");
//    cerr<<"launched kernel\n";
//    if (debug){
//     err = commandQueue->enqueueReadBuffer(*buffer_inv_xx, CL_TRUE, 0, models*subsetsize2*sizeof(float),inv_xx);
//      clSafe(err, "read inv_xx");
//      cerr<<"Read buffer\n";
//      for(int model=0;model<models;++model){
//        cerr<<"Inversion GPU:\n";
//        for(int j=0;j<subsetsize;++j){
//          for(int k=0;k<subsetsize;++k){
//            cerr<<" "<<inv_xx[model*subsetsize2+j*subsetsize+k];
//          }
//          cerr<<endl;
//        }
//      }
//    }
//    if(benchmark) event->wait();
//    cerr<<"invert GPU time: "<<(clock()-start)/CLOCKS_PER_SEC<<endl;
//  }
//  if (debug) exit(0);
//}

//void compute_xxx(){
//  bool debug = false;
//  int subsetsize2 = subsetsize*subsetsize;
//  int blockstride = (SSVS_SMALL_BLOCK_WIDTH);
//  float source_block[subsetsize*blockstride];
//  float dest_block[subsetsize*blockstride];
//  if (run_cpu){
//    for(int model=0;model<models;++model){
//      for(int chunk=0;chunk<samplesize/SSVS_SMALL_BLOCK_WIDTH+1;++chunk){
//        // LOAD THE CURRENT SSVS_BLOCK
//        memset(source_block,0,sizeof(float)*subsetsize*blockstride);
//        memset(dest_block,0,sizeof(float)*subsetsize*blockstride);
//        for(int j=0;j<subsetsize;++j){
//          for(int threadindex=0;threadindex<SSVS_SMALL_BLOCK_WIDTH;++threadindex){
//            int personindex = chunk*SSVS_SMALL_BLOCK_WIDTH+threadindex;
//            if (personindex<samplesize){
//              source_block[j*blockstride+threadindex] = designmat[subset[model*subsetsize+j]*samplesize+personindex];
//            }
//          }
//        }
//        for(int row=0;row<subsetsize;++row){
//          for(int col=0;col<subsetsize;++col){
//            for(int threadindex = 0;threadindex<SSVS_SMALL_BLOCK_WIDTH;++threadindex){
//              dest_block[row*blockstride+threadindex]+=inv_xx[model*subsetsize2+row*subsetsize+col]
//              * source_block[col*blockstride+threadindex];
//  
//            }
//          }
//        }  
//        for(int j=0;j<subsetsize;++j){
//          for(int threadindex=0;threadindex<SSVS_SMALL_BLOCK_WIDTH;++threadindex){
//            int personindex = chunk*SSVS_SMALL_BLOCK_WIDTH+threadindex;
//            if (personindex<samplesize){
//              xxx[model*(subsetsize*samplesize)+j*samplesize+personindex] = 
//              dest_block[j*blockstride+threadindex]; 
//            }
//          }
//        }
//      }
//      if (debug){
//        cerr<<"XXX CPU:\n";
//        for(int j=0;j<subsetsize;++j){
//          for(int i=0;i<samplesize;++i){
//            if (i==(samplesize-100))cerr<<" "<<xxx[model*(subsetsize*samplesize)+j*samplesize+i];
//          }
//          cerr<<endl;
//        }
//      }
//    }
//  }
//  if (run_gpu){
//   err = commandQueue->enqueueNDRangeKernel(*kernel_compute_xxx,cl::NullRange,cl::NDRange(models*SSVS_BLOCK_WIDTH),cl::NDRange(SSVS_BLOCK_WIDTH),NULL,NULL);
//    clSafe(err,"launch xxx");
//    cerr<<"launched kernel\n";
//    if (debug){
//      //err = commandQueue->enqueueWriteBuffer(*buffer_xxx, CL_TRUE, 0,  models*(subsetsize*samplesize)*sizeof(float), xxx, NULL, NULL );
//      //clSafe(err, "write xxx");
//     err = commandQueue->enqueueReadBuffer(*buffer_xxx, CL_TRUE, 0, models*(subsetsize*samplesize)*sizeof(float),xxx);
//      clSafe(err, "read xxx");
//      cerr<<"Read buffer\n";
//      for(int model=0;model<models;++model){
//        cerr<<"XXX GPU:\n";
//        for(int j=0;j<subsetsize;++j){
//          for(int i=0;i<samplesize;++i){
//            //cerr<<" "<<xxx[model*(subsetsize*samplesize)+j*samplesize+i];
//            if (i==(samplesize-100))cerr<<" "<<xxx[model*(subsetsize*samplesize)+j*samplesize+i];
//          }
//          cerr<<endl;
//        }
//      }
//    }
//  }
//  if (debug) exit(0);
//}

void SSVS::compute_beta(){
  bool debug = false;
  //float threadblock[SSVS_SMALL_BLOCK_WIDTH];
  if (run_cpu){
    double start = clock();
    int blockstride = (SSVS_SMALL_BLOCK_WIDTH);
    //float source_block[subsetsize*blockstride];
    float dest_block[subsetsize*blockstride];
    memset(beta,0,models*subsetsize*sizeof(float));
    for(int model=0;model<models;++model){
      for(int chunk=0;chunk<samplesize/SSVS_SMALL_BLOCK_WIDTH+1;++chunk){
//        // LOAD THE CURRENT SSVS_BLOCK
        //memset(source_block,0,sizeof(float)*subsetsize*blockstride);
        memset(dest_block,0,sizeof(float)*subsetsize*blockstride);
        //for(int j=0;j<subsetsize;++j){
        //  for(int threadindex=0;threadindex<SSVS_SMALL_BLOCK_WIDTH;++threadindex){
        //    int personindex = chunk*SSVS_SMALL_BLOCK_WIDTH+threadindex;
        //    if (personindex<samplesize){
        //      source_block[j*blockstride+threadindex] = designmat[subset[model*subsetsize+j]*samplesize+personindex];
        //    }
        //  }
       // }
        for(int row=0;row<subsetsize;++row){
          for(int col=0;col<subsetsize;++col){
            for(int threadindex = 0;threadindex<SSVS_SMALL_BLOCK_WIDTH;++threadindex){
              int personindex = chunk*SSVS_SMALL_BLOCK_WIDTH+threadindex;
              dest_block[row*blockstride+threadindex]+=inv_xx[model*subsetsize2+row*subsetsize+col]
             // * source_block[col*blockstride+threadindex];
              * designmat[subset[model*subsetsize+col]*samplesize+personindex];
  
            }
          }
        }  
        for(int j=0;j<subsetsize;++j){
          for(int threadindex=0;threadindex<SSVS_SMALL_BLOCK_WIDTH;++threadindex){
            int personindex = chunk*SSVS_SMALL_BLOCK_WIDTH+threadindex;
            if (personindex<samplesize){
              beta[model*subsetsize+j]+=dest_block[j*blockstride+threadindex]*trait[personindex]; 
            }
          }
        }
      }
      //for(int row=0;row<subsetsize;++row){
      //  for(int chunk=0;chunk<samplesize/SSVS_SMALL_BLOCK_WIDTH+1;++chunk){
      //    memset(threadblock,0,sizeof(float)*SSVS_SMALL_BLOCK_WIDTH);
      //    for(int threadindex=0;threadindex<SSVS_SMALL_BLOCK_WIDTH;++threadindex){
      //      int personindex = chunk*SSVS_SMALL_BLOCK_WIDTH+threadindex;
      //      if (personindex<samplesize){
      //        threadblock[threadindex] = xxx[model*(subsetsize*samplesize)+row*samplesize+personindex] * trait[personindex];
      //      }
      //    }
      //    for(int threadindex=0;threadindex<SSVS_SMALL_BLOCK_WIDTH;++threadindex){
      //      beta[model*subsetsize+row]+=threadblock[threadindex];
      //    }
      //  }
     // }
      if(debug){
        cerr<<"beta CPU:";
        //for(int j=0;j<1;++j){
        for(int j=0;j<subsetsize;++j){
          cerr<<" "<<subset[model*subsetsize+j]<<","<<beta[model*subsetsize+j];
        }
          cerr<<endl;
      }
    }
    if (benchmark) cerr<<"beta CPU time: "<<(clock()-start)/CLOCKS_PER_SEC<<endl;
  }
  if (run_gpu){
    //err = commandQueue->enqueueWriteBuffer(*buffer_xxx, CL_TRUE, 0,  sizeof(float)*subsetsize*samplesize,xxx, NULL, NULL );
    //clSafe(err, "write xxx");
    double start = clock();
   err = commandQueue->enqueueNDRangeKernel(*kernel_compute_beta,cl::NullRange,cl::NDRange(models*SSVS_BLOCK_WIDTH),cl::NDRange(SSVS_BLOCK_WIDTH),NULL,event);
    clSafe(err,"launch beta");
    if (debug){
      cerr<<"launched kernel\n";
     err = commandQueue->enqueueReadBuffer(*buffer_beta, CL_TRUE, 0, models*subsetsize*sizeof(float),beta);
      clSafe(err, "read beta");
      cerr<<"Read buffer\n";
      for(int model=0;model<models;++model){
        cerr<<"beta GPU:";
        //for(int j=0;j<1;++j){
        for(int j=0;j<subsetsize;++j){
          cerr<<" "<<beta[model*subsetsize+j];
        }
        cerr<<endl;
      }
    }
    if(benchmark) {
      event->wait();
      cerr<<"beta GPU time: "<<(clock()-start)/CLOCKS_PER_SEC<<endl;
    }
  }
  if (debug) exit(0);
}

void SSVS::compute_rsq(){
  bool debug = false;
  if(run_cpu){
    double start = clock();
    for(int model=0;model<models;++model){
      float fitted[samplesize];
      memset(fitted,0,sizeof(float)*samplesize);
      for(int var=0;var<subsetsize;++var){
        for(int personchunk=0;personchunk<samplesize/SSVS_BLOCK_WIDTH+1;++personchunk){
          for(int threadindex=0;threadindex<SSVS_BLOCK_WIDTH;++threadindex){
            int personindex = personchunk*SSVS_BLOCK_WIDTH+threadindex;
            if (personindex<samplesize){
              fitted[personindex]+=beta[model*subsetsize+var]*designmat[subset[model*subsetsize+var]*samplesize+personindex];    
            }
          }
        }
      }
      if (debug){
        //cerr<<"fitted CPU:\n";
        for(int j=samplesize-10;j<samplesize;++j){
          //cerr<<" "<<fitted[j];
          //cerr<<endl;
        }
      }
      float resid[samplesize];
      for(int personchunk=0;personchunk<samplesize/SSVS_BLOCK_WIDTH+1;++personchunk){
        for(int threadindex=0;threadindex<SSVS_BLOCK_WIDTH;++threadindex){
          int personindex = personchunk*SSVS_BLOCK_WIDTH+threadindex;
          if (personindex<samplesize){
            //resid[personindex] = fitted[personindex]-trait_mean;
            resid[personindex] = trait[personindex] - fitted[personindex];
          }
        }
      }
      float rss = 0;
      for(int i=0;i<samplesize;++i){
        rss+=resid[i]*resid[i];
      }
      //for(int i=0;i<10;++i) cerr<<" fit:"<<fitted[i];
      //cerr<<endl;
      //cerr<<"traitmean: "<<trait_mean<<",RSS CPU: "<<rss<<endl;
      rsq_arr[model] = 1.-rss/tss;
      if (debug) cerr<<model<<":R2 CPU: "<<rsq_arr[model]<<endl;
      float likelihood = -.5*samplesize*(1.837877+log(1.*rss/(samplesize-subsetsize))+1);
      if (debug) cerr<<model<<":Likelihood CPU: "<<likelihood<<endl;
    }
    if (benchmark) cerr<<"rsq CPU time: "<<(clock()-start)/CLOCKS_PER_SEC<<endl;
  }
  if (run_gpu){
    double start = clock();
    //err = commandQueue->enqueueWriteBuffer(*buffer_xxx, CL_TRUE, 0,  sizeof(float)*subsetsize*samplesize,xxx, NULL, NULL );
    //clSafe(err, "write xxx");
   err = commandQueue->enqueueNDRangeKernel(*kernel_compute_rsq,cl::NullRange,cl::NDRange(models*SSVS_BLOCK_WIDTH,1),cl::NDRange(SSVS_BLOCK_WIDTH,1),NULL,event);
    clSafe(err,"launch rsq");
   err = commandQueue->enqueueReadBuffer(*buffer_rsq, CL_TRUE, 0, models*sizeof(float),rsq_arr);
    clSafe(err, "read rsq");
    if (debug){
      cerr<<"Read buffer\n";
      for(int model=0;model<models;++model){
        cerr<<model<<":R2 GPU: "<<rsq_arr[model]<<"\n";
        for(int j=0;j<subsetsize;++j){
          cerr<<" "<<subset[model*subsetsize+j];
        }
        cerr<<endl;
      }
    }
    if(benchmark) {
      event->wait();
      cerr<<"rsq GPU time: "<<(clock()-start)/CLOCKS_PER_SEC<<endl;
    } 
  }
}

void SSVS::get_random_subset(int * subset){
  //for(int i=0;i<subsetsize;++i){
  //  subset[i] = i+10;
  //}
  //return;
  
  set<int> seen;
  //seen.insert(0);
  //subset[0] = 0;
  for(int i=0;i<subsetsize;++i){
    int selected = 0;
    bool ok;
    do{
      float r =  math->RandomUniform() * grandsum;
      prefixsum_element e;
      e.sum = r;
      e.index = 0;
      prefixsum_set_type::iterator it =  prefixsum_set.lower_bound(e);
      selected = it->index;
      ok = seen.find(selected)==seen.end();
    }while (!ok);
    subset[i] = selected;
    //cerr<<"Var: "<<selected<<endl;
    seen.insert(selected);
  }
}

void SSVS::update_prefixsum(){
  grandsum = 0;
  for(int i=0;i<variables;++i){
    grandsum+=weight_vector[i];
    prefixsum_element e;
    e.sum = grandsum;
    e.index = i;
    prefixsum_set.insert(e);
  }
}

SSVS::SSVS(){
}

void SSVS::init(const ptree & pt){
  MPI_Init(0,NULL);
  MPI_Comm_size(MPI_COMM_WORLD,&mpi_numtasks);
  MPI_Comm_rank(MPI_COMM_WORLD,&mpi_rank);
  if (mpi_rank){
    ostringstream oss;
    oss<<"debug.slave."<<mpi_rank;
    ofs.open(oss.str().data());
  }else{
    ostringstream oss;
    oss<<"debug.master";
    ofs.open(oss.str().data());
  }
  math = new MathUtils(mpi_rank);
  settings = new ssvs_settings_t;
  benchmark = settings->benchmark = pt.get<int>("benchmark");
  good_model_threshold = settings->good_model_threshold = pt.get<int>("good_model_threshold");
  iterations = settings->iterations = pt.get<int>("iterations");
  samplesize = settings->samplesize = pt.get<int>("samplesize");
  variables = settings->variables = pt.get<int>("variables");
  if (mpi_rank==0) cerr<<"Samplesize: "<<samplesize<<" Variables: "<<variables<<endl;
  models = settings->models = pt.get<int>("models");
  subsetsize = settings->subsetsize = pt.get<int>("subsetsize");

  subsetsize2 = subsetsize*subsetsize ;
  previous_weight_vector = new float[variables];
  weight_vector = new float[variables];
  rsq_arr = new float[models];
  for(int i=0;i<variables;++i) weight_vector[i] = 
  previous_weight_vector[i]=1.;
  trait = new float[samplesize];
  designmat = new float[samplesize*variables];

  subset = new int[models*subsetsize];
  good_model = new int[models*subsetsize];
  good_likelihoods = new float[models];
  xx = new float[models*subsetsize2];
  inv_xx = new float[models*subsetsize2];
  //xxx = new float[models*subsetsize*samplesize];
  beta = new float[models*subsetsize];

  ifstream ifs_design("designmat");
  if (!ifs_design.is_open()) throw "Can't open design mat input";
  for(int i=0;i<samplesize;++i){
    string line;
    getline(ifs_design,line);
    istringstream iss(line);
    for(int j=0;j<variables;++j){
      iss>>designmat[j*samplesize+i];
    }
  }
  ifs_design.close();
  trait_mean = 0;
  ifstream ifs_trait("trait");
  if (!ifs_trait.is_open()) throw "Can't open trait mat input";
  for(int i=0;i<samplesize;++i){
    string line;
    getline(ifs_trait,line);
    istringstream iss(line);
    iss>>trait[i];
    trait_mean+=trait[i];
  }
  ifs_trait.close();
  trait_mean/=samplesize;
  tss = 0;
  for(int i=0;i<samplesize;++i){
    tss+=pow(trait[i]-trait_mean,2);
  }
  //cerr<<"trait_mean: "<<trait_mean<<" TSS "<<tss<<endl;
  BOOST_FOREACH(boost::property_tree::ptree::value_type host_object, pt.get_child("opencl_settings")){
    boost::property_tree::ptree host_tree = (boost::property_tree::ptree)host_object.second;
    int hostid = host_tree.get<int>("<xmlattr>.rank");
    if (hostid==mpi_rank){
      settings->use_gpu = !host_tree.get<string>("enable_opencl").compare("true")?true:false;
      settings->platform_id = host_tree.get<int>("platform_id");
      settings->device_id = host_tree.get<int>("device_id");
      ofs<<"This process wants device id: "<<settings->device_id<<endl;
      settings->kernel_path = host_tree.get<string>("kernel_path");
    }
  }
  // set up MPI data structures
  MPI_Type_contiguous(variables,MPI_INT,&VectorIntArrayType);
  MPI_Type_commit(&VectorIntArrayType);
  MPI_Type_contiguous(variables,MPI_FLOAT,&VectorFloatArrayType);
  MPI_Type_commit(&VectorFloatArrayType);

  if (settings->use_gpu){
    int platform_id = settings->platform_id;
    int device_id = settings->device_id;
    const char * kernel_path=settings->kernel_path.data();
    init_gpu(&platform_id,&device_id,kernel_path);
    init_buffers();
  }
  run_gpu = true;
  run_cpu = false;
}

void SSVS::sample_variables(){
  update_prefixsum();
  //cerr<<"prefix sum complete\n";
  for(int m=0;m<models;++m){
    //cerr<<"Model "<<m<<endl;
    get_random_subset(subset+m*subsetsize);
  }
  //cerr<<"Random vars sampled\n";
#ifdef USE_GPU
 err = commandQueue->enqueueWriteBuffer(*buffer_subset, CL_TRUE, 0,  sizeof(int)*models*subsetsize,subset, NULL, NULL );
  clSafe(err, "write subset");
#endif
  //cerr<<"models transferred\n";
}

void SSVS::fit_models(){
  invert_xprimex();
  //invert();
  compute_beta();
  compute_rsq();
}

void SSVS::get_top_likelihood(){
  float top_likelihood = -9000;
  if(mpi_rank==0){
    for(int dest=1;dest<mpi_numtasks;++dest){
      float cur_like;
      rc = MPI_Recv(&cur_like,1,MPI_FLOAT,dest,TAG_SYNC_MAX_LIKELIHOOD1,MPI_COMM_WORLD,&stat);
      if (cur_like>top_likelihood) top_likelihood = cur_like;
    }
    // PROMOTE THE LOWER OF THE BEST LIKELIHOODS
    if (top_likelihood < alltime_best_likelihood){
      top_likelihood = alltime_best_likelihood;
    }else if (top_likelihood > alltime_best_likelihood){
      alltime_best_likelihood = top_likelihood;
    }
    for(int dest=1;dest<mpi_numtasks;++dest){
      rc = MPI_Send(&top_likelihood,1,MPI_FLOAT,dest,TAG_SYNC_MAX_LIKELIHOOD2,MPI_COMM_WORLD);
    }
    //cerr<<"Top likelihood found was: "<<top_likelihood<<endl;
  }else{
    for(int model=0;model<models;++model){
      if (rsq_arr[model]>top_likelihood) top_likelihood = rsq_arr[model];
    }
    rc = MPI_Send(&top_likelihood,1,MPI_FLOAT,0,TAG_SYNC_MAX_LIKELIHOOD1,MPI_COMM_WORLD);
    rc = MPI_Recv(&top_likelihood,1,MPI_FLOAT,0,TAG_SYNC_MAX_LIKELIHOOD2,MPI_COMM_WORLD,&stat);
  }
  current_best_likelihood = top_likelihood;
}

void SSVS::update_weight_vector(){
  if (mpi_rank==0){
    float unif = 1./variables;
    float alpha = .5;
    for(int model=0;model<good_models;++model){
      float denom = 1.;
      for(int j=0;j<subsetsize;++j){
        int var = good_model[model*subsetsize+j];
        denom*=alpha*(unif)+(1.-alpha)*(previous_weight_vector[var]);
      }
      for(int j=0;j<subsetsize;++j){
        int var = good_model[model*subsetsize+j];
        weight_vector[var] += unif/denom;
      }
    }
    for(int v=0;v<variables;++v){
      previous_weight_vector[v] = weight_vector[v];
    }
  }
  return;
}

bool SSVS::acceptreject_models(){
  int accept;
  if(mpi_rank==0){
    good_models = 0;
    int slavemodels[mpi_numtasks-1];
    for(int dest=1;dest<mpi_numtasks;++dest){
      int slave = dest-1;
      int models;
      rc = MPI_Recv(&models,1,MPI_INT,dest,TAG_GOOD_MODELS,MPI_COMM_WORLD,&stat);
      slavemodels[slave] = models;
      good_models+=models;
      //cerr<<"Slave "<<slave<<" reporting "<<models<<" models.\n";
    }
    accept = good_models>=good_model_threshold;
    for(int dest=1;dest<mpi_numtasks;++dest){
      rc = MPI_Send(&accept,1,MPI_INT,dest,TAG_ACCEPT_CODE,MPI_COMM_WORLD);
    }
  }else{
    // CLEAN UP THE LINKED LIST AND REMOVE MODELS THAT FAIL DUE 
    // TO UPDATED BEST LIKELIHOOD
    list<model_t>::iterator it=candidate_models.begin();
   //cerr<<mpi_rank<<" Searching list of size "<<candidate_models.size()<<"\n";
    while(it!=candidate_models.end()){
      float modelprob = exp(it->likelihood-current_best_likelihood);
      //cerr<<"Model variables:";
      vector<int> variables = it->variables;
      for(int j=0;j<subsetsize;++j){
        //cerr<<" "<< variables[j];
      }
      //cerr<<endl;
      if (modelprob<math->RandomUniform()){
        cerr<<"Rejected old model with likelihood "<< it->likelihood << " and prob "<<modelprob<<" best: "<<current_best_likelihood<<endl;
        it = candidate_models.erase(it);
      }else{
        it++;
      }
   }
   //cerr<<mpi_rank<<" Searched list\n";
    // ADD TO THE LINKED LIST GOOD MODELS
    for(int model=0;model<models;++model){
      float modelprob = (rsq_arr[model]>current_best_likelihood)?1
      :exp(rsq_arr[model]-current_best_likelihood);
      if (modelprob>math->RandomUniform()){
        model_t modelobj(subsetsize,subset+model*subsetsize,rsq_arr[model]);
        candidate_models.insert(candidate_models.begin(),modelobj);
      }else{
        //cerr<<"Rejected new model with likelihood "<<rsq_arr[model]<<" and prob "<<modelprob<<" best: "<<current_best_likelihood<<endl;
      }
    }
    good_models = candidate_models.size();
    rc = MPI_Send(&good_models,1,MPI_INT,0,TAG_GOOD_MODELS,MPI_COMM_WORLD);
    rc = MPI_Recv(&accept,1,MPI_INT,0,TAG_ACCEPT_CODE,MPI_COMM_WORLD,&stat);
  }
  return accept;
}

void SSVS::sync_good_models(){
  if(mpi_rank==0){
    good_models = 0;
    int slavemodels[mpi_numtasks-1];
    for(int dest=1;dest<mpi_numtasks;++dest){
      int slave = dest-1;
      int models;
      rc = MPI_Recv(&models,1,MPI_INT,dest,TAG_GOOD_MODELS,MPI_COMM_WORLD,&stat);
      slavemodels[slave] = models;
      good_models+=models;
      cerr<<"Slave "<<slave<<" reporting "<<models<<" models.\n";
    }
    float likelihoodarr[good_models];
    int offset = 0;
    for(int dest=1;dest<mpi_numtasks;++dest){
      int slave = dest-1;
      rc = MPI_Recv(likelihoodarr+offset,slavemodels[slave],MPI_FLOAT,dest,TAG_GOOD_MODEL_LIKELIHOOD,MPI_COMM_WORLD,&stat);
      rc = MPI_Recv(good_model+(offset*subsetsize),slavemodels[slave]*subsetsize,MPI_INT,dest,TAG_GOOD_MODEL,MPI_COMM_WORLD,&stat);
      offset += slavemodels[slave];
    }
    cerr<<"Good models: "<<good_models<<endl;
    for(int model=0;model<good_models;++model){
      cerr<<"Model "<<model<<" variables:";
      for(int j=0;j<subsetsize;++j){
        cerr<<" "<< good_model[model*subsetsize+j];
      }
      cerr<<endl;
    }
  }else{
    good_models = 0;
    for(list<model_t>::iterator it=candidate_models.begin();
    it!=candidate_models.end();it++){
      good_likelihoods[good_models] = it->likelihood;
      vector<int> variables = it->variables;
      for(int j=0;j<subsetsize;++j){
        good_model[good_models*subsetsize+j] = variables[j];
      }
      ++good_models;
    }
    rc = MPI_Send(&good_models,1,MPI_INT,0,TAG_GOOD_MODELS,MPI_COMM_WORLD);
    rc = MPI_Send(good_likelihoods,good_models,MPI_FLOAT,0,TAG_GOOD_MODEL_LIKELIHOOD,MPI_COMM_WORLD);
    rc = MPI_Send(good_model,good_models*subsetsize,MPI_INT,0,TAG_GOOD_MODEL,MPI_COMM_WORLD);
  }
}

void SSVS::sync_weight_vector(){
  if(mpi_rank==0){
    for(int dest=1;dest<mpi_numtasks;++dest){
      rc = MPI_Send(weight_vector,1,VectorFloatArrayType,dest,TAG_SYNC_POSTERIOR,MPI_COMM_WORLD);
      rc = MPI_Send(&current_best_likelihood,1,MPI_FLOAT,dest,TAG_SYNC_MAX_LIKELIHOOD1,MPI_COMM_WORLD);
    }
  }else{
    rc = MPI_Recv(weight_vector,variables,MPI_FLOAT,0,TAG_SYNC_POSTERIOR,MPI_COMM_WORLD,&stat);
    rc = MPI_Recv(&current_best_likelihood,1,MPI_FLOAT,0,TAG_SYNC_MAX_LIKELIHOOD1,MPI_COMM_WORLD,&stat);
  }
}

void SSVS::list_posteriors(){
  update_prefixsum();
  ofstream ofs("posteriors");
  for(int j=0;j<variables;++j){
    //if (weight_vector[j]>1){
    ofs<<" "<<j<<":"<<weight_vector[j];
    //}
  }
  ofs<<endl;
  ofs.close();
}

SSVS::~SSVS(){

}

void SSVS::run(){
  //SSVS * ssvs = new SSVS();
  try{
    for(int iter = 0;iter < iterations;++iter){
      bool accept = false;
      alltime_best_likelihood = -9000;
      good_models = 0;
      cerr<<"Iteration : "<<iter<<endl;
        // ROUTINE TO GENERATE RANDOM SUBSETS
        //cerr<<"Syncing Posteriors\n";
      sync_weight_vector();
      candidate_models.clear();
      while(!accept){
        if (mpi_rank>0){
          //cerr<<"Sampling variables\n";
          sample_variables();
          //cerr<<"Fitting models\n";
          fit_models();
        }
        //cerr<<"Syncing models\n";
        get_top_likelihood();
        accept = acceptreject_models();
        //cerr<<"finished accept/reject\n";
        //delete ssvs;
      }
      sync_good_models();
      update_weight_vector();
    }
    if(mpi_rank==0){
      list_posteriors();
    }
    cerr<<"Finalizing MPI\n";
    MPI_Finalize();
    ofs.close();
  }catch (const char *  & str){
    cerr<<"Exception caught: "<<str<<endl;
  }
}

int main2(){
  return 0;
}

